/********************************** (C) COPYRIGHT *******************************
* File Name          : motor.c
* Author             : nx666
* Version            : V1.0.0
* Date               : 2023/07/26
* Description        : 电机驱动器的主文件.
*******************************************************************************/
/* 头文件区域 */
#include "motor.h"
#include <string.h>
#include "debug.h"
#include "FreeRTOS.h"
#include "task.h"
#include "modbus.h"
#include "foc.h"
#include "interact.h"

/*  类型定义区  */
typedef struct motor
{
    foc_   *foc;
    pid_   *pidSpeed;
    uInt16 *regs;
    motorParam_ param;
    Int (*setSpeed)(struct motor *,uInt16);
    Int (*speedLoop)(struct motor *);
}motor_;


/* 宏定义区 */


/* 全局变量区 */

/*********************************************************************
 * @fn      motor_setSpeed
 *
 * @brief   实现电流环，
 *
 * @return  FUNC_SOK，成功  FUNC_FAIL，失败
 *          
 */
static Int motor_setSpeed(struct motor *pMotor,uInt16 v)
{
    if (pMotor->param.omega_w != _IQ8(v))
    {
        pMotor->param.omega_w = _IQ8(v);
    }
    return 0;
}

/*********************************************************************
 * @fn      foc_curruntLoop
 *
 * @brief   实现电流环，
 *
 * @return  FUNC_SOK，成功  FUNC_FAIL，失败
 *          
 */
static Int motor_speedLoop(struct motor *pMotor)
{
    // smo_             *pSmo;
    _iq               omega;
    _iq               err;
    _iq               dstIq;
    static _iq        localOmega_w = _IQ8(OPEN_LOOP_OMEGA);

    // pSmo = pMotor->foc->smo;

    //获取速度
    // omega = pSmo->getOmega(pSmo);
    pMotor->param.omega = _IQ8mpy(omega,180) + _IQ8mpy(pMotor->param.omega,76); //lpf = 0.7
    omega = pMotor->param.omega;

    //计算差值
    localOmega_w = _IQ8mpy(pMotor->param.omega_w,76) + _IQ8mpy(localOmega_w,180); //lpf = 0.3
    err = localOmega_w - omega;

    //pid
    dstIq = pMotor->pidSpeed->calc(pMotor->pidSpeed, err);
    if (dstIq > ID_IQ_LIMIT)
    {
        dstIq = ID_IQ_LIMIT;
    }
    else if (dstIq < -ID_IQ_LIMIT)
    {
        dstIq = -ID_IQ_LIMIT;
    }
    //设置速度
    //pMotor->foc->setIq(pMotor->foc, -256);
    pMotor->foc->setIdIq(pMotor->foc, 0, 800);
    // printMat[0] = (float)(pMotor->param.omega_w);
    // printMat[1] = (float)(omega);
    // printMat[2] = (float)(dstIq);

    // printData(printMat,3);
    return 0;
}


/*********************************************************************
 * @fn      thread_motor
 *
 * @brief   motor主线程，
 *              void
 *
 * @return  FUNC_SOK，成功  FUNC_FAIL，失败
 *          
 */
Int thread_motor(void *arg)
{
    uInt32 count = 0;
    motor_ *pMotor;
    printf("%s  -- start\n",__FUNCTION__);

    pMotor = (motor_ *)malloc(sizeof(motor_));
    if (!pMotor)
    {
        return -1;
    }
    memset((void *)pMotor, 0, sizeof(motor_));

    pMotor->setSpeed = motor_setSpeed;
    pMotor->speedLoop = motor_speedLoop;

    //pMotor->pidSpeed = pidCreate(_IQ12(0.04), _IQ12(0.008), _IQ12(0.024)); //60 - 155
    //pMotor->pidSpeed = pidCreate(_IQ12(0.01), _IQ12(0.008), _IQ12(0.084));  //155 - 195
    //pMotor->pidSpeed = pidCreate(_IQ12(0.04), _IQ12(0.038), _IQ12(0.024));

    pMotor->pidSpeed = pidCreate(1, 1, _IQ12(0.0));

    pMotor->regs = modbus_slave_init(0x59);
    pMotor->foc = focCreate();

    //设置速度环目标速度
    pMotor->param.omega_w = _IQ8(OPEN_LOOP_OMEGA);

    //设置开环参数
    pMotor->foc->setStatus(pMotor->foc, OPEN_LOOP);
    pMotor->regs[REG_UD] = 1;
    pMotor->regs[REG_UQ] = 15;
    pMotor->regs[REG_OMEGA] = OPEN_LOOP_OMEGA;
    
    pMotor->foc->setUdUq(pMotor->foc, _IQ8div(_IQ8(pMotor->regs[REG_UD]),_IQ8(10)),_IQ8div(_IQ8(pMotor->regs[REG_UQ]),_IQ8(10)));
    pMotor->foc->setOmega(pMotor->foc, _IQ8div(_IQ8(pMotor->regs[REG_OMEGA]),_IQ8(1000000 / FOC_CALC_CYCLE)));
    //pMotor->foc->setIdIq(pMotor->foc,_IQ8div(_IQ8(0),_IQ8(10)),_IQ8div(_IQ8(0),_IQ8(10)));
    pMotor->foc->setIdIq(pMotor->foc, 0, 300);

    while(1)
    {
        modbus_recv_bytes();
        modbus_event();
    
        if ( 15000 == count )
        {
            pMotor->foc->setStatus(pMotor->foc, OPEN_LOOP);
        }

        if ((count > 5000)&&(0 == (count % 500)))
        {
            pMotor->setSpeed(pMotor,pMotor->regs[REG_OMEGA]);
            pMotor->speedLoop(pMotor);
        }
        count++;

        Delay_Ms(1);
    }
    vTaskDelete(NULL);
    return 0;
}
